// Fill out your copyright notice in the Description page of Project Settings.
#include "DataIndicatorMeshBuilder.h"
#include "DynamicMesh/DynamicMesh3.h"
#include "DynamicMeshToMeshDescription.h" 
#include "UDynamicMesh.h"
using UE::Geometry::FIndex3i;
using UE::Geometry::FDynamicMesh3;

void UDataIndicatorMeshBuilder::BuildDataIndicatorMesh(
    UDynamicMesh* targetMesh, const FVector& startPos, const FVector& secPos, const FVector& endPos,
    const FVector& startNormal, const FVector& endNormal,
    const float width, const float UTexcoordStart, 
    FVector& nextstartPos, float& nextUTexcoordStart, const bool& isLastMesh)
{
    if (!targetMesh)
        return;

    FVector dir, dir2;
    float length, length2;
    const FVector p1(startPos), p2(secPos);
	(p2 - p1).ToDirectionAndLength(dir, length);
	FVector tangent = (startNormal ^ dir).GetSafeNormal();

    targetMesh->EditMesh([&](FDynamicMesh3& Mesh)
    {

            Mesh.Clear();
            // Below is a situation when the result of crossing the vector p1p2 and the vector p2p3 is < 0.
            //       -->Forward
            // 0 ------vb0----- 2 ----- v4
            // |        |              . |
            // |        |            .   |
            // |        |          .     |   Line vb1-v4 is angle bisector.
            // p1 ------------- p2       |   pf2 is the center point of line vb0-v4.
            // |        |     . |        |   From p2 to 3 is another direction.
            // |        |   .   |        |
            // |        | .     |        |
            // 1 ------vb1----- 3 ----- v5
            // 
            // Below is a situation when the result of crossing the vector p1p2 and the vector p2p3 is > 0.
            //       -->Forward
            // 0 ------vb0----- 2 ----- v5
            // |        | .     |        |
            // |        |   .   |        |
            // |        |     . |        |   Line vb0-v4 is angle bisector.
            // p1 ------------- p2       |   pf2 is the center point of line vb0-v5.
            // |        |          .     |   From p2 to 2 is another direction.
            // |        |            .   |
            // |        |              . |
            // 1 ------vb1----- 3 ----- v4

            // vertex positions:
            const FVector widthOffset(tangent * (width * 0.5f));
            const FVector v0(p1 + widthOffset);
            const FVector v1(p1 - widthOffset);
            const FVector v2(p2 + widthOffset);
            const FVector v3(p2 - widthOffset);
            if (!isLastMesh)
            {
                const FVector p3(endPos);
                (p3 - p2).ToDirectionAndLength(dir2, length2);

                const FVector angleBisector((dir - dir2).GetSafeNormal());
                const FVector p2p1((p1 - p2).GetSafeNormal());
                const float sinTheta = (angleBisector ^ p2p1).Size();
                const float cosTheta = sqrtf(1 - sinTheta * sinTheta);
                const float alpha = width * 0.5f * cosTheta / sinTheta;
                const FVector pstandard(alpha * p2p1);
                const FVector vb0(pstandard + v2);
                const FVector vb1(pstandard + v3);
                const FVector pf2(p2 + dir2 * alpha);
                FVector v4, v5;
                const float indicator = ((p2 - p1) ^ (p3 - p2)).Z;
                if (indicator < 0)
                {
                    v4 = 2 * v2 - vb0;
                    v5 = 2 * pf2 - vb1;
                }
                else
                {
                    v4 = 2 * v3 - vb1;
                    v5 = 2 * pf2 - vb0;
                }
                float offset_ = 10;
                const FVector vbb0(vb0 - offset_ * dir);
                const FVector vbb1(vb1 - offset_ * dir);
                const FVector vf5(v5 + offset_ * dir2);
                const FVector pff2(pf2 + offset_ * dir2);
                const FVector vf(2 * pff2 - vf5);

                //get next mesh start position
                nextstartPos = pff2;
                // index:
                int32 vi0 = Mesh.AppendVertex(v0);
                int32 vi1 = Mesh.AppendVertex(v1);
                int32 vi2 = Mesh.AppendVertex(vbb0);
                int32 vi3 = Mesh.AppendVertex(vbb1);
                int32 vi4, vi5, vi6, vi7;
                if (indicator < 0)
                {
                    vi4 = Mesh.AppendVertex(v4);
                    vi5 = Mesh.AppendVertex(vb1);
                    vi6 = Mesh.AppendVertex(vf5);
                    vi7 = Mesh.AppendVertex(vf);
                }
                else
                {
                    vi4 = Mesh.AppendVertex(vb0);
                    vi5 = Mesh.AppendVertex(v4);
                    vi6 = Mesh.AppendVertex(vf);
                    vi7 = Mesh.AppendVertex(vf5);
                }
                // tirangle:
                Mesh.AppendTriangle(vi0, vi2, vi3);
                Mesh.AppendTriangle(vi0, vi3, vi1);
                Mesh.AppendTriangle(vi2, vi4, vi5);
                Mesh.AppendTriangle(vi2, vi5, vi3);
                Mesh.AppendTriangle(vi4, vi6, vi7);
                Mesh.AppendTriangle(vi4, vi7, vi5);
                // UV:
                if (!Mesh.HasAttributes())
                {
                    Mesh.EnableAttributes();
                }
                auto* UVOverlay = Mesh.Attributes()->PrimaryUV();
                FString screenMessage;
                if (UVOverlay)
                {
                    FVector2f moveStart(FVector2f(UTexcoordStart, 0.0f));
                    FVector2f move(FVector2f((vbb0 - v0).Size(), 0.0f));
                    int32 uv0 = UVOverlay->AppendElement(FVector2f(0.0f, width) + moveStart);
                    int32 uv1 = UVOverlay->AppendElement(FVector2f(0.0f, 0.0f) + moveStart);
                    int32 uv2 = UVOverlay->AppendElement(FVector2f(0.0f, width) + move + moveStart);
                    int32 uv3 = UVOverlay->AppendElement(FVector2f(0.0f, 0.0f) + move + moveStart);

                    int n = 3;
                    int32 uv4 = UVOverlay->AppendElement(FVector2f(width * n / 2, width) + move + moveStart);
                    int32 uv5 = UVOverlay->AppendElement(FVector2f(width * n / 2, 0.0f) + move + moveStart);
                    int32 uv6 = UVOverlay->AppendElement(FVector2f(width * n, width) + move + moveStart);
                    int32 uv7 = UVOverlay->AppendElement(FVector2f(width * n, 0.0f) + move + moveStart);
                    UVOverlay->SetTriangle(0, FIndex3i(uv0, uv2, uv3));
                    UVOverlay->SetTriangle(1, FIndex3i(uv0, uv3, uv1));
                    UVOverlay->SetTriangle(2, FIndex3i(uv2, uv4, uv5));
                    UVOverlay->SetTriangle(3, FIndex3i(uv2, uv5, uv3));
                    UVOverlay->SetTriangle(4, FIndex3i(uv4, uv6, uv7));
                    UVOverlay->SetTriangle(5, FIndex3i(uv4, uv7, uv5));

                    nextUTexcoordStart = move.X + width + moveStart.X;
                }
                // normal:
                auto* NormalOverlay = Mesh.Attributes()->PrimaryNormals();
                if (NormalOverlay)
                {
                    FVector3f startNormalF(startNormal);
                    FVector3f ensNormalF(endNormal);

                    int32 n0 = NormalOverlay->AppendElement(startNormalF);
                    int32 n1 = NormalOverlay->AppendElement(startNormalF);
                    int32 n2 = NormalOverlay->AppendElement(ensNormalF);
                    int32 n3 = NormalOverlay->AppendElement(ensNormalF);
                    int32 n4 = NormalOverlay->AppendElement(ensNormalF);
                    int32 n5 = NormalOverlay->AppendElement(ensNormalF);

                    NormalOverlay->SetTriangle(0, FIndex3i(n0, n2, n3));
                    NormalOverlay->SetTriangle(1, FIndex3i(n0, n3, n1));
                    NormalOverlay->SetTriangle(2, FIndex3i(n2, n3, n5));
                    NormalOverlay->SetTriangle(3, FIndex3i(n2, n4, n5));
                    NormalOverlay->SetTriangle(4, FIndex3i(n2, n3, n5));
                    NormalOverlay->SetTriangle(5, FIndex3i(n2, n4, n5));
                    NormalOverlay->SetTriangle(6, FIndex3i(n2, n4, n5));
                }
            }
            else
            {
                // index:
                int32 vi0 = Mesh.AppendVertex(v0);
                int32 vi1 = Mesh.AppendVertex(v1);
                int32 vi2 = Mesh.AppendVertex(v2);
                int32 vi3 = Mesh.AppendVertex(v3);
                // tirangle:
                Mesh.AppendTriangle(vi0, vi2, vi3);
                Mesh.AppendTriangle(vi0, vi3, vi1);
                // UV:
                if (!Mesh.HasAttributes())
                {
                    Mesh.EnableAttributes();
                }

                auto* UVOverlay = Mesh.Attributes()->PrimaryUV();
                if (UVOverlay)
                {
                    int32 uv0 = UVOverlay->AppendElement(FVector2f(UTexcoordStart, width));
                    int32 uv1 = UVOverlay->AppendElement(FVector2f(UTexcoordStart, 0.0f));
                    int32 uv2 = UVOverlay->AppendElement(FVector2f(UTexcoordStart + length, width));
                    int32 uv3 = UVOverlay->AppendElement(FVector2f(UTexcoordStart + length, 0.0f));

                    UVOverlay->SetTriangle(0, FIndex3i(uv0, uv2, uv3));
                    UVOverlay->SetTriangle(1, FIndex3i(uv0, uv3, uv1));
                }

                // normal:
                auto* NormalOverlay = Mesh.Attributes()->PrimaryNormals();
                if (NormalOverlay)
                {
                    FVector3f startNormalF = (FVector3f)startNormal;
                    FVector3f ensNormalF = (FVector3f)endNormal;

                    int32 n0 = NormalOverlay->AppendElement(startNormalF);
                    int32 n1 = NormalOverlay->AppendElement(startNormalF);
                    int32 n2 = NormalOverlay->AppendElement(ensNormalF);
                    int32 n3 = NormalOverlay->AppendElement(ensNormalF);

                    NormalOverlay->SetTriangle(0, FIndex3i(n0, n2, n3));
                    NormalOverlay->SetTriangle(1, FIndex3i(n0, n3, n1));
                }
            }
    }, EDynamicMeshChangeType::GeneralEdit, EDynamicMeshAttributeChangeFlags::Unknown);
}